#ifndef __TOOL_HPP__
#define __TOOL_HPP__
#include <math.h>

struct quaternion
{
    float w;
    float x;
    float y;
    float z;
}; 


quaternion * yaw2quaternion(double yaw)
{ 
    float radian=yaw/180.0 * 3.1415926;


    int n[3]= { 0,0,1 };
    float half=radian/2;
    float _cos=cos(half);
    float _sin=sin(half);


    float w = _cos;
    float x = _sin * n[0];
    float y = _sin * n[1];
    float z = _sin * n[2];

    if(abs(w)<0.001)  w=0;
    if(abs(x)<0.001) x=0;
    if(abs(y)<0.001) y=0;
    if(abs(z)<0.001) z=0;

    quaternion * q = new quaternion;
    q->w = w;
    q->x = x;
    q->y = y;
    q->z = z;
    return q;
}



#endif